import FCMathUtil

from maya import OpenMaya as OM



class Steering:
    def __init__( self ):
        self.side = OM.MVector()
        self.up = OM.MVector()
        self.forward = OM.MVector()
        self.position = OM.MVector()
        
        self.wanderSide = 0
        self.wanderUp = 0
        
        self.hisPositionAtNearestApproach = OM.MVector()
        self.ourPositionAtNearestApproach = OM.MVector()
        
        self.mass = 1
        self.speed = 0
        self.radius = 0.5
        self.maxForce = 0.1
        self.maxSpeed = 1.0
        
        self.curvature = 0
        self.smoothedCurvature = 0.0
        
        self.lastForward = OM.MVector()
        self.lastPosition = OM.MVector()
        
        self.smoothedPosition = OM.MVector()
        self.smoothedAcceleration = OM.MVector()
        
        self.reset()
        
    def reset( self ):
        self.forward = OM.MVector.zAxis
        self.side = self.localRotateForwardToSide( self.forward )
        self.up = OM.MVector.yAxis
        self.position = OM.MVector.zero
        
        self.wanderSide = 0
        self.wanderUp = 0
        
        self.mass = 1
        self.speed = 0
        self.radius = 0.5
        self.maxForce = 0.1
        self.maxSpeed = 1.0
        
        self.resetSmoothedPosition()
        self.resetSmoothedCurvature()
        self.resetSmoothedAcceleration()
    
    def rightHanded( self ):
        return True
    
    def localizeDirection( self, _GlobalDirection ):
        return OM.MVector( _GlobalDirection * self.side, _GlobalDirection * self.up, _GlobalDirection * self.forward )
        
    def globalizeDirection( self, _LocalDirection ):
        return (self.side * _LocalDirection.x) + (self.up * _LocalDirection.y) + (self.forward * _LocalDirection.z)
    
    def localizePosition( self, _GlobalPosition ):
        return self.localizeDirection( _GlobalPosition - self.position )
        
    def globalizePosition( self, _LocalPosition ):
        return self.position + self.globalizeDirection( _LocalPosition )
    
    def setUnitSideFromFrowardAndUp( self ):
        if self.rightHanded():
            self.side = self.forward ^ self.up
        else:
            self.side = self.up ^ self.forward
            
        self.side.normalize()
    
    def regenerateOrthonormalBasisUF( self, _NewUnitForward ):
        self.forward = _NewUnitForward
        self.setUnitSideFromFrowardAndUp()
        
        if self.rightHanded():
            self.up = self.side ^ self.forward
        else:
            self.up = self.forward ^ self.side
        
    def regenerateOrthonormalBasis( self, _NewUnitForward ):
        self.regenerateOrthonormalBasisUF( _NewUnitForward.normal() )
        
    def regenerateOrthonormalBasis( self, _NewForward, _NewUp ):
        self.up = _NewUp
        self.regenerateOrthonormalBasis( _NewForward.normal() )        
    
    def localRotateForwardToSide( self, _vector ):
        if self.rightHanded():
            return OM.MVector( -_vector.z, _vector.y, _vector.z )
        else:
            return OM.MVector( _vector.z, _vector.y, _vector.z )

    def globalRotateForwardToSide( self, _GlobalForward ):
        _LocalForward = self.localizeDirection( _GlobalForward )
        _LocalSide = self.localRotateForwardToSide( _LocalForward )
        return self.globalizeDirection( _LocalSide )
    
    
        
    def steerForWander( self, _Dt ):
        _Speed = 100 * _Dt
        self.wanderSide = FCMathUtil.scalarRandomWalk( self.wanderSide, _Speed, -1, 1 )
        self.wanderUp = FCMathUtil.scalarRandomWalk( self.wanderUp, _Speed, -1, 1 )
        
        return (self.base.side * self.wanderSide) + (self.base.up * self.wanderUp)
    
    def steerForSeek( self, _Target ):
        _Offset = self.base.position - _Target
        _DesiredVelocity = FCMathUtil.truncateLength( _Offset, self.base.maxSpeed )
        return _DesiredVelocity - self.base.position
    
    def steerForFlee( self, _Target ):
        _Offset = _Target - self.base.position
        _DesiredVelocity = FCMathUtil.truncateLength( _Offset, self.base.maxSpeed )
        return _DesiredVelocity - self.base.position
        
    # ------------------------------------------------------------------------
    # Unaligned collision avoidance behavior: avoid colliding with other
    # nearby vehicles moving in unconstrained directions.  Determine which
    # (if any) other other vehicle we would collide with first, then steers
    # to avoid the site of that potential collision.  Returns a steering
    # force vector, which is zero length if there is no impending collision.
    def steerToAvoidNeighbors( self, _MinTimeToCollision, _Others ):
        _Separation = self.steerToAvoidCloseNeighbors( 0, _Others )
        if not FCMathUtil.isZeroVector( _Separation ): return _Separation
        
        _Steer = 0
        _Threat = None
        _MinTime = _MinTimeToCollision
        
        _ThreatPositionAtNearestApproach = OM.MVector()
        _OurPositionAtNearestApproach = OM.MVector()
        
        for _Vehicle in _Others:
            if self.serialNumber == _Vehicle.serialNumber: continue
            
            _CollisionDangerThreshold = self.radius * 2
            _Time = self.predictNearestApproachTime( _Others )
            if (0 <= _Time) and (_Time < _MinTime):
                if self.computeNearestApproachPositions( _Vehicle, _Time ) < _CollisionDangerThreshold:
                    _MinTime = _Time
                    _Threat = _Vehicle
                    _ThreatPositionAtNearestApproach = self.hisPositionAtNearestApproach
                    _OurPositionAtNearestApproach = self.ourPositionAtNearestApproach
                    
        if None != _Threat:
            _Parallelness = self.forward * _Threat.forward
            _Angle = 0.707
            
            if _Parallelness < -_Angle:
                _Offset = _ThreatPositionAtNearestApproach - self.position
                _SideDot = _Offset * self.side
                _Steer = 1.0
                if _SideDot > 0: _Steer = -1.0
            else:
                if _Parallelness > _Angle:
                    _Offset = _Threat.position - self.position
                    _SideDot = _Offset * self.side
                    _Steer = 1.0
                    if _SideDot > 0: _Steer = -1.0
                else:
                    if _Threat.speed <= self.speed:
                        _SideDot = self.side * _Threat.velocity()
                        _Steer = 1.0
                        if _SideDot > 0: _Steer = -1.0
                        
        return self.side * _Steer
        
    # Given two vehicles, based on their current positions and velocities,
    # determine the time until nearest approach
    def predictNearestApproachTime( self, _Other ):
        _MyVelocity = self.velocity()
        _OtherVelocity = _Other.velocity()
        _RelVelocity = _OtherVelocity - _MyVelocity
        _RelSpeed = _RelVelocity.length()
        if _RelSpeed == 0: return 0
        
        _RelTangent = _RelVelocity / _RelSpeed
        _RelPosition = self.position - _Other.position
        _Projection = _RelTangent * _RelPosition
        
        return _Projection / _RelSpeed
    
    # Given the time until nearest approach (predictNearestApproachTime)
    # determine position of each vehicle at that time, and the distance
    # between them
    def computeNearestapproachPositions( self, _Other, _Time ):
        _MyTravel = self.forward * self.speed * _Time
        _MyFinal = self.position + _MyTravel
        
        _OtherTravel = _Other.forward * _Other.speed * _Time
        _OtherFinal = _Other.position + _OtherFinal
        
        self.ourPositionAtNearestApproach = _MyFinal
        self.hisPositionAtNearestApproach = _OtherFinal
        
        return distance( _MyFinal, _OtherFinal )
        
    # ------------------------------------------------------------------------
    # avoidance of "close neighbors" -- used only by steerToAvoidNeighbors
    #
    # Does a hard steer away from any other agent who comes withing a
    # critical distance.  Ideally this should be replaced with a call
    # to steerForSeparation.
    def steerToAvoidCloseNeighbors( self, _MinSeparationDistance, _Others ):
        for _Other in _Others:
            if self.serialNumber == _Other.serialNumber: continue
            
            _SumOfRadii = self.radius + _Other.radius
            _MinCenterToCenter = _MinSeparationDistance + _SumOfRadii
            _Offset = _Other.position - self.position
            _CurrentDistance = _Offset.length()
            
            if _CurrentDistance < _MinCenterToCenter:
                return FCMathUtil.perpendicularComponent( -_Offset, self.forward )
                
        return MVector.zero
        
    def isAhead( self, _Target ): return self.isAhead( _Target, 0.707 )
    def isAside( self, _Target ): return self.isAside( _Target, 0.707 )
    def isBehind( self, _Target ): return self.isBehind( _Target, -0.707 )
    
    def isAhead( self, _Target, _CosThreshold ):
        _TargetDirection = (_Target - self.base.position).normal()
        return (self.base.forward * _TargetDirection) > _CosThreshold
        
    def isAside( self, _Target, _CosThreshold ):
        _TargetDirection = (_Target - self.base.position).normal()
        _Dp = self.base.forward * _TargetDirection
        return ( _Dp < _CosThreshold ) and ( _Dp > -_CosThreshold )
        
    def isBehind( self, _Target, _CosThreshold ):
        _TargetDirection = (_Target - self.base.position).normal()
        return (self.base.forward * _TargetDirection) < _CosThreshold


    def resetSmoothedPosition( self, _Vector = OM.MVector.zero ):
        self.smoothedPosition.x = _Vector.x
        self.smoothedPosition.y = _Vector.y
        self.smoothedPosition.z = _Vector.z
        return self.smoothedPosition
        
    def resetSmoothedCurvature( self, _Value = 0 ):
        self.lastForward = OM.MVector.zero
        self.lastPosition = OM.MVector.zero
        self.curvature = _Value
        self.smoothedCurvature = _Value
        return self.smoothedCurvature
        
    def resetSmoothedAcceleration( self, _Vector = MVector.zero ):
        self.smoothedAcceleration.x = _Vector.x
        self.smoothedAcceleration.y = _Vector.y
        self.smoothedAcceleration.z = _Vector.z
        return self.smoothedAcceleration
        
    def velocity( self ):
        return self.forward * self.speed
    
    def adjustRawSteeringForce( self, _Force, _Dt ):
        _MaxAdjustedSpeed = 0.2 * self.maxSpeed
        if (self.speed > _MaxAdjustedSpeed) or FCMathUtil.isZeroVector( _Force ):
            return _Force
        else:
            _Range = self.speed / _MaxAdjustedSpeed
            _Cosine = FCMathUtil.interpolate( pow( _Range, 20 ), 1.0, -1.0 )
            return FCMathUtil.limitMaxDeviationAngle( _Force, _Cosine, self.forward )
        
    def applySteeringForce( self, _Force, _Dt ):
        _AdjustedForce = self.adjustRawSteeringForce( _Force, _Dt )
        _ClippedForce = FCMathUtil.truncateLength( _AdjustedForce, self.maxForce )
        
        _NewAcceleration = _ClippedForce / self.mass
        _NewVelocity = self.velocity()
        
        if 0 < _Dt:
            _SmoothRate = clamp( 9 * _Dt, 0.15, 0.5 )
            self.smoothedAcceleration = FCMathUtil.blendIntoAccumulator( _SmoothRate, _NewAcceleration, self.smoothedAcceleration )
            
        _NewVelocity += self.smoothedAcceleration * _Dt
        _NewVelocity = FCMathUtil.truncateLength( _NewVelocity, self.maxSpeed )
        
        self.speed = _NewVelocity.length()
        self.position = self.position + ( _NewVelocity * _Dt )
        
        self.regenerateLocalSpace( _NewVelocity, _Dt )
        self.measurePathCurvature( _Dt )
        
        self.smoothedPosition = FCMathUtil.blendIntoAccumulator( _Dt * 0.06, self.position, self.smoothedPosition )
        
    def applyBrakingForce( _Rate, _Dt ):
        _ClipBraking = _RawBraking = self.speed * _Rate
        if _RawBraking < self.maxForce:
            _ClipBraking = self.maxForce
            
        self.speed = self.speed - (_ClipBraking * _Dt)
    
    def predictNextPosition( self, _PredictionTime ):
        return self.position + (self.velocity * _PredictionTime )
        
    def regenerateLocalSpace( self, _NewVelocity, _Dt ):
        if 0 < self.speed: self.regenerateOrthonormalBasisUF( _NewVelocity / self.speed )
    
    def regenerateLocalSpaceForBanking( self, _NewVelocity, _Dt ):
        _GlobalUp = OM.MVector( 0, 0.2, 0 )
        _AccelUp = self.smoothedAcceleration * 0.05
        _BankUp = _AccelUp + _GlobalUp
        _SmoothRate = _Dt * 3
        _TempUp = self.up
        _TempUp = blendIntoAccumulator( _SmoothRate, _BankUp, _TempUp )
        self.up = _TempUp.normal()
        
        if 0 < self.speed: self.regenerateOrthonormalBasisUF( _NewVelocity / self.speed )
        
    def measurePathCurvature( self, _Dt ):
        if 0 < _Dt:
            _DeltaP = self.lastPosition - self.position
            _DeltaF = (self.lastForward - self.forward) / _DeltaP.length()
            _Lateral = FCMathUtil.perpendicularComponent( _DeltaF, self.forward )
            _Sign = -1.0
            if _Lateral * self.side: _Sign = 1.0
            
            self.curvature = _Lateral.length() * _Sign
            self.smoothedCurvature = FCMathUtil.blendIntoAccumulator( _Dt * 4.0, self.curvature, self.smoothedCurvature )
            
            self.lastForward = self.forward
            self.lastPosition = self.position
            
    def randomizeHeadingOnZXPlane( self ):
        self.up = OM.MVector.zero
        self.forward = FCMathUtil.randomUnitVectorOnXZPlane()
        self.side = self.localRotateForwardToSide( self.forward )